#include "visualization/perception_visualizer.h"

#include <chrono>
#include <functional>
#include <limits>
#include <opencv2/core/eigen.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/opencv.hpp>

#include "common/utility.h"

namespace autodrive {
namespace perception {

PerceptionVisualizer::PerceptionVisualizer()
    : is_dispose_{false}, is_init_{false}, is_close_{false} {}

PerceptionVisualizer::~PerceptionVisualizer() { Close(); }

PerceptionVisualizer& PerceptionVisualizer::GetInstance() noexcept {
  static PerceptionVisualizer instance;
  return instance;
}

void PerceptionVisualizer::Init() noexcept {
  auto initFun{[&]() {
    if (nullptr == viewer_buffer_ptr_) {
      viewer_buffer_ptr_ = std::make_shared<ViewerBuffer>();
    }

    viewer_task_ = std::thread{view_task, this};
  }};
  std::call_once(init_flag_, initFun);
  is_init_ = true;
}

void PerceptionVisualizer::update_display_image(
    PerceptionResult const* result_ptr, cv::Mat& img) noexcept {
  if (nullptr != result_ptr) {
    cv::Size rawImageSize{704, 256};
    std::int32_t imageRows{5};
    std::int32_t imageCols{2};

    img = cv::Mat(cv::Size(rawImageSize.width * imageCols,
                           rawImageSize.height * imageRows),
                  CV_8UC3, cv::Scalar(50, 50, 50));

    auto dispImg{[&result_ptr, &img, &rawImageSize](
                     std::int32_t index, std::int32_t x, std::int32_t y,
                     std::string const& caption) {
      auto matROI{img(cv::Rect(x, y, rawImageSize.width, rawImageSize.height))};

      if (nullptr != result_ptr->image_source_ptr &&
          result_ptr->image_source_ptr->images.size() > index) {
        auto const& img{result_ptr->image_source_ptr->images.at(index)};
        img.copyTo(matROI);
      } else {
        cv::Mat img(rawImageSize, CV_8UC3, cv::Scalar(255, 0, 0));
        img.copyTo(matROI);
      }

      std::int32_t offset{30};
      cv::putText(img, caption, cv::Point2i(x + offset, y + offset),
                  cv::FONT_HERSHEY_SIMPLEX, 0.75, cv::Scalar(0, 0, 255), 2);
    }};

    std::int32_t offset{rawImageSize.width / 2};
    dispImg(0, 0, 0, "front");
    dispImg(6, rawImageSize.width, 0, "front crop");

    // fisheye
    dispImg(1, offset, rawImageSize.height, "front fisheye");
    dispImg(2, 0, rawImageSize.height * 2, "left fisheye");
    dispImg(4, rawImageSize.width, rawImageSize.height * 2, "right fisheye");
    dispImg(3, offset, rawImageSize.height * 3, "back fisheye");

    dispImg(5, offset, rawImageSize.height * 4, "back");

    cv::rectangle(img,
                  cv::Rect(0, rawImageSize.height, rawImageSize.width * 2,
                           rawImageSize.height * 3),
                  cv::Scalar(50, 255, 50), 4);
  }
}

pcl::visualization::PCLVisualizer::Ptr
PerceptionVisualizer::gen_pcl_viewer() noexcept(false) {
  auto pclVisPtr{pcl::visualization::PCLVisualizer::Ptr(
      new pcl::visualization::PCLVisualizer("Perception Viewer"),
      [](pcl::visualization::PCLVisualizer* ptr) {
        if (nullptr != ptr) {
          // ptr->close();
        }
      })};

  if (pclVisPtr == nullptr) {
    throw std::runtime_error("pcl visualizer create fail");
  }

  pclVisPtr->initCameraParameters();
  pclVisPtr->setCameraPosition(0, 0, 200, 1, 0, 0);
  pclVisPtr->setShowFPS(true);

  pclVisPtr->setBackgroundColor(0.15, 0.15, 0.15);
  std::string const& e2eID{"E2E"};
  pclVisPtr->addText("E2E", 10, 15, 25, 1, 1, 1, e2eID);

  pclVisPtr->addCoordinateSystem(1.0);

  /// self car
  std::string const& selfCarID{"SELFCAR"};
  pclVisPtr->addCube(Eigen::Vector3f{0, 0, 0}, Eigen::Quaternionf::Identity(),
                     4.2, 1.7, 1.6, selfCarID);

  pclVisPtr->setShapeRenderingProperties(
      pcl::visualization::PCL_VISUALIZER_REPRESENTATION,
      pcl::visualization::PCL_VISUALIZER_REPRESENTATION_WIREFRAME, selfCarID);
  pclVisPtr->setShapeRenderingProperties(
      pcl::visualization::PCL_VISUALIZER_COLOR, 1, 0, 0, selfCarID);

  return pclVisPtr;
}

std::string PerceptionVisualizer::gen_image_viewer() noexcept {
  std::string id{"CameraWindow"};
  cv::namedWindow(id, cv::WindowFlags::WINDOW_NORMAL);
  cv::resizeWindow(id, cv::Size2i(640, 480));
  return id;
}

void PerceptionVisualizer::update_timestamp(
    pcl::visualization::PCLVisualizer::Ptr vis_ptr,
    std::uint64_t const& timestamp) noexcept {
  if (vis_ptr != nullptr) {
    std::string const& timestampID{"TIMESTAMP"};
    auto const& strTimestamp{std::to_string(timestamp)};
    if (!vis_ptr->contains(timestampID)) {
      vis_ptr->addText(strTimestamp, 100, 10, 25, 1, 1, 0, timestampID);

    } else {
      vis_ptr->updateText(strTimestamp, 100, 10, timestampID);
    }
  }
}

void PerceptionVisualizer::update_pcl_source(
    pcl::visualization::PCLVisualizer::Ptr vis_ptr,
    PointCloudSource const* pcl_source_ptr) noexcept {
  std::uint8_t r{255};
  std::uint8_t g{255};
  std::uint8_t b{255};
  auto const& pclSouceID{"PCLSOUCE"};

  if (nullptr == point_cloud_source_ptr_) {
    point_cloud_source_ptr_ =
        pcl::make_shared<pcl::PointCloud<pcl::PointXYZRGB>>();
  }
  point_cloud_source_ptr_->clear();
  if (nullptr != pcl_source_ptr) {
    for (auto const& p : pcl_source_ptr->points) {
      point_cloud_source_ptr_->emplace_back(
          pcl::PointXYZRGB{p.x, p.y, p.z, r, g, b});
    }
  }
  if (nullptr != vis_ptr) {
    if (!vis_ptr->contains(pclSouceID)) {
      pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(
          point_cloud_source_ptr_);
      vis_ptr->addPointCloud<pcl::PointXYZRGB>(point_cloud_source_ptr_, rgb,
                                               pclSouceID);
      vis_ptr->setPointCloudRenderingProperties(
          pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, pclSouceID);
    } else {
      vis_ptr->updatePointCloud<pcl::PointXYZRGB>(point_cloud_source_ptr_,
                                                  pclSouceID);
    }
  }
}

void PerceptionVisualizer::view_task(
    PerceptionVisualizer* const vis_ptr) noexcept {
  try {
    if (nullptr == vis_ptr) {
      throw std::runtime_error("vis_ptr is nullptr");
    }

    auto pclVisPtr{vis_ptr->gen_pcl_viewer()};

    auto imageViewerID{vis_ptr->gen_image_viewer()};

    std::uint64_t currentTimestamp{0};

    cv::Mat imageDisplay(cv::Size(640, 480), CV_8UC3, cv::Scalar(255, 0, 0));

    while (true) {
      try {
        if (vis_ptr->is_close_ || vis_ptr->IsDispose()) {
          std::cout << "[" << to_yellow("Warr") << "]"
                    << " PerceptionVisualizer command to stop ..." << std::endl;
          return;
        }

        if (nullptr == vis_ptr->viewer_buffer_ptr_) {
          throw std::runtime_error("viewer_buffer_ptr_ is nullptr");
        }

        PerceptionResult const* resultPtr{nullptr};
        if (vis_ptr->viewer_buffer_ptr_->GetNumFilled() > 0) {
          auto resultPair{vis_ptr->viewer_buffer_ptr_->GetLatest()};
          if (nullptr != resultPair.second && nullptr != *resultPair.second) {
            resultPtr = *resultPair.second;
          }
        }

        // display image
        auto displayImage{[&vis_ptr, &resultPtr, &imageViewerID, &imageDisplay,
                           &currentTimestamp]() {
          if (nullptr != resultPtr &&
              currentTimestamp != resultPtr->timestamp) {
            vis_ptr->update_display_image(resultPtr, imageDisplay);
          }

          cv::imshow(imageViewerID, imageDisplay);
          cv::waitKey(10);
        }};

        std::shared_future<void> dispImgFuture{
            vis_ptr->display_image_task_.Run(displayImage)};

        // pcl display
        auto pclDisplay{[&vis_ptr, &pclVisPtr, &resultPtr,
                         &currentTimestamp]() {
          if (nullptr != resultPtr &&
              currentTimestamp != resultPtr->timestamp) {
            vis_ptr->update_timestamp(pclVisPtr, currentTimestamp);
            vis_ptr->update_pcl_source(pclVisPtr, resultPtr->pcl_source_ptr);
          }

          if (!pclVisPtr->wasStopped()) {
            pclVisPtr->spinOnce(10);
          } else {
            vis_ptr->is_dispose_ = true;
          }
        }};

        pclDisplay();

        // sync
        auto const& timeWait{200};
        if (dispImgFuture.wait_for(std::chrono::milliseconds(timeWait)) ==
            std::future_status::timeout) {
          std::cout << "[" << to_yellow("Error") << "]"
                    << "display camera images timeout!!!" << '\n';
        } else {
          dispImgFuture.get();
        }

        if (nullptr != resultPtr) {
          currentTimestamp = resultPtr->timestamp;
        }

      } catch (const std::exception& e) {
        std::cout << "[" << to_red("Error") << "]"
                  << "PerceptionVisualizer update display error, " << e.what()
                  << '\n';
      } catch (...) {
        std::cout << "[" << to_red("Error") << "]"
                  << "PerceptionVisualizer update display error, unexpected"
                  << '\n';
      }
    }
  } catch (const std::exception& e) {
    if (vis_ptr != nullptr) {
      vis_ptr->is_dispose_ = true;
    }
    std::cout << "[" << to_red("Error") << "]"
              << "PerceptionVisualizer run error, " << e.what() << '\n';
  } catch (...) {
    if (vis_ptr != nullptr) {
      vis_ptr->is_dispose_ = true;
    }
    std::cout << "[" << to_red("Error") << "]"
              << "PerceptionVisualizer run error, unexpected!" << '\n';
  }
}

void PerceptionVisualizer::Enqueue(PerceptionResult const& result) noexcept(
    false) {
  std::lock_guard<std::mutex> lock(lock_);
  if (!IsInit()) {
    throw std::runtime_error("PerceptionVisualizer is not init");
  }

  if (nullptr == viewer_buffer_ptr_) {
    throw std::runtime_error("viewer_buffer_ptr_ is nullptr");
  }

  viewer_buffer_ptr_->Append(&result);
}

void PerceptionVisualizer::Close() noexcept {
  std::lock_guard<std::mutex> lock(lock_);
  if (!is_close_) {
    cv::destroyAllWindows();
    is_close_ = true;
    if (viewer_task_.joinable()) {
      viewer_task_.join();
    }
  }
}

}  // namespace perception
}  // namespace autodrive
